'''
Created on Dec 10, 2009

@author: Peter Brook
'''
import numpy as np
import MathUtil as mu
import math
from copy import copy
class GraspTestTrajectoryGenerator(object):
    '''
    Generates trajectory arrays for a grasp to allow the grasp to be
    played back on the robot, as well as tested.
    '''


    def __init__(self, initialJoints, timestep=0.1):
        '''
        Constructor
        '''
        self.initialJoints = initialJoints
        self.timestep = timestep
        
    def compute_pouring_trajectory(self, timeToExecute):
        '''
        Computes the intermediate joint angles for a characteristic 
        "pouring" motion of the wrist.
        '''
        finalJoints = copy(self.initialJoints)
        finalJoints[6] += math.pi / 2 #rotate last joint 90 degrees
        joints_array = mu.ndlerp(self.initialJoints, finalJoints, 
                                  timeToExecute/self.timestep)
        reversed = copy(joints_array)
        reversed.reverse()
        return joints_array+reversed
    
    def compute_hand_over_trajectory(self, timeToExecute):
        '''
        Computes the intermediate joint angles for a "hand over" motion path
        
        Keyword arguments:
        timeToExecute -- the time that the trajectory should take to complete on 
        the robot
        First, we plan the path to a predefined point that marks the "start"
        of our path. Once we are at that start point, we execute a predefined
        trajectory taking us from that point to our "destination" point. 
        '''
        hand_over_intermediate_joints = np.array([ 0.,0.83948147,0.61403537,2.1616509 ,-0.03249407,-1.57079661, -1.82306695], dtype='float32')
        final_position = np.array([-1.05644929,1.0839994,0.61403537,1.97419071,-0.7745961,-1.28892529,-1.91438675], dtype='float32')
        
        initial_segment = mu.ndlerp(self.initialJoints, hand_over_intermediate_joints, timeToExecute/self.timestep)
        second_segment = mu.ndlerp(hand_over_intermediate_joints, final_position, 3/self.timestep)
        
        return initial_segment+second_segment
    def generate_trajectory_file(self,filename, data):
        f = file(filename,'w')
        for line in data:
            txt = "0, "+", ".join(map(str,line))
            f.write(txt+"\n")
        f.close()
